//
//  main.cpp
//  DCA
//
//  Created by James Critchley on 7/26/11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"

#include "DCFlowControl.h"
#include "DCAComputation.h"

#include "Timer.h"

void *ThreadFunction(void *s)
{        
    DCFlowControl *dca = (DCFlowControl*) s;
    while(1){
//             dca->kinAssembly_traversal();
//             dca->kinSolve_traversal();
//             dca->EOM_traversal();            
            dca->assembly_traversal();                
            dca->solve_traversal();
    }
    return 0;
}

int main (int argc, const char * argv[])
{        
        const double& q = 1.0;
        const double& u = 1.0;
        
        const int& nbodies = 1024;
        const int& nproc = 4;
        const int& niter = 1;
        int x,y,z;
        x = 4;
        y = nbodies - 3*x;
        z = 1024/4;
        
        RSIM::Sys3D mbs3D(nbodies,nproc);
        mbs3D.initDCANodesForDCAABA();        
        mbs3D.setQ(q);
        mbs3D.setU(u);
        
        DCBinaryParallelFlow dca0,dca11,dca12;        
        DCFlowTerminator dca21,dca22,dca23,dca24;
                
        dca0.child1 = &dca11;dca0.child2 = &dca12;
        
        dca11.child1 = &dca21;dca11.child2 = &dca22;
        
        dca12.child1 = &dca23;dca12.child2 = &dca24;

        CommSync sync_0_12, sync_11_22, sync_12_24;
        
        dca0.child_sync = dca12.parent_sync = &sync_0_12;
        dca11.child_sync = dca22.parent_sync = &sync_11_22;
        dca12.child_sync = dca24.parent_sync = &sync_12_24;

        // declair the calculations
        DCARoot comp0;
        DCABasic comp11, comp12;        
        DCAABA comp21,comp22,comp23,comp24;
        
        // link the computations
        dca0.computation = &comp0;
        dca11.computation = &comp11;
        dca12.computation = &comp12;
        dca21.computation = &comp21;
        dca22.computation = &comp22;
        dca23.computation = &comp23;
        dca24.computation = &comp24;
        
        // associate local data with the computations
        comp0.mbs3D = &mbs3D;
        comp0.parent_id = 4;
        comp0.child_id = 5;
        comp0.asm_id = 6;  
                
        comp11.mbs3D = &mbs3D;
        comp11.parent_id = 0;
        comp11.child_id = 1;
        comp11.asm_id = 4;  
        comp11.calcH2 = true;        

        comp12.mbs3D = &mbs3D;
        comp12.parent_id = 2;
        comp12.child_id = 3;
        comp12.asm_id = 5;

        int id_count = 0;
        comp21.mbs3D = &mbs3D;
        comp21.start_id = 0;    comp21.end_id = x-1;      comp21.asm_id = 0;
        comp21.eom_startid = 0; comp21.eom_endid = z-1;
        comp21.kin_startid = 0; comp21.kin_endid = y-1;
        comp21.calcH2 = true;
        mbs3D.ids_[id_count++] = comp21.kin_startid;
                
        comp22.mbs3D = &mbs3D;
        comp22.start_id = x;    comp22.end_id = 2*x-1;     comp22.asm_id = 1;
        comp22.eom_startid = z; comp22.eom_endid = 2*z-1;
        comp22.kin_startid = y; comp22.kin_endid = y+x-1;       
        comp22.calcH2 = true;
        mbs3D.ids_[id_count++] = comp22.kin_startid;                        

        comp23.mbs3D = &mbs3D;
        comp23.start_id = 2*x;   comp23.end_id = 3*x-1;     comp23.asm_id = 2;
        comp23.eom_startid = 2*z; comp23.eom_endid = 3*z-1;
        comp23.kin_startid = y+x; comp23.kin_endid = y+2*x-1;
        comp23.calcH2 = true;
        mbs3D.ids_[id_count++] = comp23.kin_startid;                
        
        comp24.mbs3D = &mbs3D;
        comp24.start_id = 3*x;   comp24.end_id = nbodies-1;     comp24.asm_id = 3;
        comp24.eom_startid = 3*z; comp24.eom_endid = 4*z-1;
        comp24.kin_startid = y+2*x; comp24.kin_endid = y+3*x-1;        
        mbs3D.ids_[id_count++] = comp24.kin_startid;
        mbs3D.ids_[id_count++] = comp24.kin_endid+1;
                
        comp0.calcH2 = comp12.calcH2 = comp24.calcH2 = false;
                                
        pthread_attr_t attr;
        pthread_t thread1,thread2,thread3;

        pthread_attr_init(&attr);                
        pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);        
        
        //set detached so thread resources will be freed immediately upon
        // thread termination (no join needed)
        pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
                
        void *data = (void *) &dca12;
        pthread_create(&thread1, &attr, ThreadFunction, data);
        data = (void *) &dca22;
        pthread_create(&thread2, &attr, ThreadFunction, data);
        data = (void *) &dca24;
        pthread_create(&thread3, &attr, ThreadFunction, data);        
        
        //clean-up the attributes
        pthread_attr_destroy(&attr);        

        RSIM::Timer t;
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();
        
        for(int i=0;i<niter;i++)
        {        
                // run the root subsystem
//                 dca0.kinAssembly_traversal();
//                 dca0.kinSolve_traversal();
//                 dca0.EOM_traversal();
                dca0.assembly_traversal();        
                dca0.solve_traversal();                

        }    
        t.stop();        
//         t.printElapsedTime(0);                
//         cout<<setprecision(16);
//         for(int i=0;i<nbodies;++i)
//                 cout<<mbs3D.barray_[i].v_<<endl;
        mbs3D.displayUDot();      

        exit(0);
        return 0;
}
